#include <sys/mman.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <signal.h>
#include <getopt.h>
#include <time.h>
#include <pthread.h>
#include <fcntl.h>
#include <ctype.h>
#include <rtdm/udd.h>
#include <cobalt/sys/cobalt.h>
#include "udd_omap_gpio.h"

#define UDD_GPIO0 "/dev/rtdm/udd_gpio,mapper0"
#define UDD_GPIO1 "/dev/rtdm/udd_gpio,mapper1"
#define UDD_GPIO2 "/dev/rtdm/udd_gpio,mapper2"
#define UDD_GPIO3 "/dev/rtdm/udd_gpio,mapper3"

static struct gpio_bank gpio_am33xx[OMAP_GPIO_BANK_NUM] = {
    {0, UDD_GPIO0, -1, NULL },
    {1, UDD_GPIO1, -1, NULL },
    {2, UDD_GPIO2, -1, NULL },
    {3, UDD_GPIO3, -1, NULL },
};

static inline int get_gpio_index(int gpio)
{
    return gpio & 0x1f;
}

int gpio_is_valid(int gpio)
{
    return (gpio >= 0) && (gpio < OMAP_MAX_GPIO);
}

static inline void _set_gpio_direction(const struct gpio_bank *bank, int gpio,
                                       int is_input)
{
    volatile void *reg = bank->base;
    uint32_t l;

    reg += OMAP_GPIO_OE;

    l = __raw_readl(reg);
    if (is_input)
        l |= 1 << gpio;
    else
        l &= ~(1 << gpio);
    __raw_writel(l, reg);
}

/**
 * Get the direction of the GPIO by reading the GPIO_OE register
 * corresponding to the specified bank.
 */
static inline int _get_gpio_direction(const struct gpio_bank *bank, int gpio)
{
    volatile void *reg = bank->base;
    uint32_t v;

    reg += OMAP_GPIO_OE;

    v = __raw_readl(reg);

    if (v & (1 << gpio))
        return OMAP_GPIO_DIR_IN;
    else
        return OMAP_GPIO_DIR_OUT;
}

static inline void _set_gpio_dataout(const struct gpio_bank *bank, int gpio,
                                     int enable)
{
    volatile void *reg = bank->base;
    uint32_t l = 0;

    if (enable)
        reg += OMAP_GPIO_SETDATAOUT;
    else
        reg += OMAP_GPIO_CLEARDATAOUT;

    l = 1 << gpio;
    __raw_writel(l, reg);
}

static inline int _get_gpio_value(const struct gpio_bank *bank, int gpio)
{
    volatile void *reg = bank->base;
    int input;

    input = _get_gpio_direction(bank, gpio);
    switch (input) {
    case OMAP_GPIO_DIR_IN:
        reg += OMAP_GPIO_DATAIN;
        break;
    case OMAP_GPIO_DIR_OUT:
        reg += OMAP_GPIO_DATAOUT;
        break;
    default:
        return -1;
    }

    return (__raw_readl(reg) & (1 << gpio)) != 0;
}

static inline const struct gpio_bank *get_gpio_bank(int gpio)
{
    return &gpio_am33xx[gpio >> 5];
}

int udd_gpio_cleanup()
{
    int i = 0;
    for (i = 0; i < OMAP_GPIO_BANK_NUM; i++) {
        close(gpio_am33xx[i].fd);
        if (gpio_am33xx[i].base != NULL) {
            munmap((void *)gpio_am33xx[i].base, 4096);
        }
    }
}

int udd_gpio_init()
{
    int i = 0;
    for (i = 0; i < OMAP_GPIO_BANK_NUM; i++) {
        if ((gpio_am33xx[i].fd = open(gpio_am33xx[i].dev, O_RDWR)) < 0) {
            perror("open");
            goto cleanup;
        }

        gpio_am33xx[i].base = (volatile unsigned int *)mmap(NULL, 4096, PROT_READ|PROT_WRITE, MAP_PRIVATE, gpio_am33xx[i].fd, 0);
        close(gpio_am33xx[i].fd);

        if (gpio_am33xx[i].base == MAP_FAILED) {
            gpio_am33xx[i].base = NULL;
            perror ("mmap ");
            goto cleanup;
        }
    }
    return 0;
cleanup:
    udd_gpio_cleanup();
    return -1;
}

static inline int check_gpio(int gpio)
{
    if (!gpio_is_valid(gpio)) {
        printf("ERROR : check_gpio: invalid GPIO %d\n", gpio);
        return -1;
    }
    return 0;
}

int gpio_clr(unsigned gpio)
{
    const struct gpio_bank *bank;

    if (check_gpio(gpio) < 0)
        return -1;

    bank = get_gpio_bank(gpio);
    volatile void *reg = bank->base;

    *(volatile unsigned int *)(reg + OMAP_GPIO_CLEARDATAOUT) = (1 << gpio);
    return 0;
}

int gpio_set(unsigned gpio)
{
    const struct gpio_bank *bank;

    if (check_gpio(gpio) < 0)
        return -1;

    bank = get_gpio_bank(gpio);
    volatile void *reg = bank->base;

    *(volatile unsigned int *)(reg + OMAP_GPIO_SETDATAOUT) = (1 << gpio);
    return 0;
}

int gpio_get(unsigned gpio)
{
    const struct gpio_bank *bank;

    if (check_gpio(gpio) < 0)
        return -1;
    bank = get_gpio_bank(gpio);

    return _get_gpio_value(bank, get_gpio_index(gpio));
}

int gpio_set_value(unsigned gpio, int value)
{
    const struct gpio_bank *bank;

    if (check_gpio(gpio) < 0)
        return -1;
    bank = get_gpio_bank(gpio);

    _set_gpio_dataout(bank, get_gpio_index(gpio), value);
    return 0;
}

/**
 * Set gpio direction as input
 */
int gpio_direction_input(unsigned gpio)
{
    const struct gpio_bank *bank;

    if (check_gpio(gpio) < 0)
        return -1;

    bank = get_gpio_bank(gpio);
    _set_gpio_direction(bank, get_gpio_index(gpio), 1);

    return 0;
}

/**
 * Set gpio direction as output
 */
int gpio_direction_output(unsigned gpio, int value)
{
    const struct gpio_bank *bank;

    if (check_gpio(gpio) < 0)
        return -1;

    bank = get_gpio_bank(gpio);
    _set_gpio_dataout(bank, get_gpio_index(gpio), value);
    _set_gpio_direction(bank, get_gpio_index(gpio), 0);

    return 0;
}

/**
 * Request a gpio before using it.
 *
 * NOTE: Argument 'label' is unused.
 */
int gpio_request(unsigned gpio, const char *label)
{
    if (check_gpio(gpio) < 0)
        return -1;

    return 0;
}

/**
 * Reset and free the gpio after using it.
 */
int gpio_free(unsigned gpio)
{
    return 0;
}


